/**
  ****************************(C) COPYRIGHT 2025 ROBOT-Z****************************
  * @file       shoot.c/h
  * @brief      2025赛季哨兵射击功能
  * @note
  * @history
  *  Version    Date            Author          Modification
  *  V1.0.0     Dec-26-2018     RM              1. 完成
  *  v2.0.0		Dec-20-2024		冉文治			1. 修改了发射逻辑
  *								卢俊烨			2. 对卡弹进行处理
  *								吴森桦
  *
  @verbatim
  ==============================================================================

  ==============================================================================
  @endverbatim
  ****************************(C) COPYRIGHT 2025 ROBOT-Z****************************
  */

#include "shoot.h"
#include "main.h"
#include "arm_math.h"
#include "user_lib.h"
#include "CAN_receive.h"
#include "gimbal_behaviour.h"
#include "pid.h"
#include "bsp_laser.h"
#include "chassis_task.h"
#include "referee.h"

#define shoot_laser_on() laser_on()
#define shoot_laser_off() laser_off()

/**
  * @brief          射击状态机设置
  * @author			冉文治
  * @param			void
  * @retval         void
  */
static void shoot_set_mode(void);

/**
  * @brief          射击数据更新
  * @author			冉文治
  * @param			void
  * @retval         void
  */
static void shoot_feedback_update(void);

/**
  * @brief          堵转处理
  * @author			卢俊烨
  * @param			void
  * @retval         void
  */
static void shoot_blocking_judgment(void);

// 射击数据
shoot_control_t shoot_control;

/**
  * @brief          射击初始化，初始化PID，遥控器指针，电机指针
  * @author			冉文治
  * @param			void
  * @retval         返回空
  */
void shoot_init(void)
{
	static const fp32 Trigger_pid_cooling_heat[3] = {TRIGGER_COOLING_HEAT_PID_KP, TRIGGER_COOLING_HEAT_PID_KI, TRIGGER_COOLING_HEAT_PID_KD};
    static const fp32 Trigger_pid_speed[3] = {TRIGGER_SPEED_PID_KP, TRIGGER_SPEED_PID_KI, TRIGGER_SPEED_PID_KD};
    static const fp32 Fric_pid_speed[3] = {FRIC_SPEED_PID_KP, FRIC_SPEED_PID_KI, FRIC_SPEED_PID_KD};
	const static fp32 fric_filter_num[1] = {2.0};
	first_order_filter_init(&shoot_control.fric_motor[0].fric_filter, GIMBAL_CONTROL_TIME, fric_filter_num);
	first_order_filter_init(&shoot_control.fric_motor[1].fric_filter, GIMBAL_CONTROL_TIME, fric_filter_num);
    ramp_init(&shoot_control.fric_ramp, GIMBAL_CONTROL_TIME, SHOOT_FRIC_MAX_SPEED, SHOOT_FRIC_MIN_SPEED);
    shoot_control.shoot_mode = SHOOT_STOP;
	shoot_control.shoot_last_mode = shoot_control.shoot_mode;

	// 遥控器指针
    shoot_control.shoot_rc = get_remote_control_point();
	// 电机指针
    shoot_control.trigger_motor.trigger_motor_measure = get_trigger_motor_measure_point();
    shoot_control.fric_motor[0].fric_motor_measure = get_fric1_motor_measure_point();
    shoot_control.fric_motor[1].fric_motor_measure = get_fric2_motor_measure_point();
	// PID初始化
	PID_init(&shoot_control.trigger_motor_pid_cooling_heat, PID_POSITION, Trigger_pid_cooling_heat, TRIGGER_COOLING_HEAT_PID_MAX_OUT, TRIGGER_COOLING_HEAT_PID_MAX_IOUT);
    PID_init(&shoot_control.trigger_motor_pid_speed, PID_POSITION, Trigger_pid_speed, TRIGGER_READY_PID_MAX_OUT, TRIGGER_READY_PID_MAX_IOUT);
    PID_init(&shoot_control.fric1_motor_pid_speed, PID_POSITION, Fric_pid_speed, FRIC_BULLET_PID_MAX_OUT, FRIC_BULLET_PID_MAX_IOUT);
    PID_init(&shoot_control.fric2_motor_pid_speed, PID_POSITION, Fric_pid_speed, FRIC_BULLET_PID_MAX_OUT, FRIC_BULLET_PID_MAX_IOUT);

	// 更新数据
    shoot_feedback_update();
}

/**
  * @brief          射击循环
  * @author			冉文治
  * @param			void
  * @retval         无
  */
void shoot_control_loop(void)
{
	// 设置模式
	shoot_set_mode();
	// 更新数据
	shoot_feedback_update();
	
	if (shoot_control.shoot_mode == SHOOT_STOP)
	{
		shoot_control.trigger_motor.trigger_speed_set = 0;
		ramp_calc(&shoot_control.fric_ramp, FRIC_CLOSE);
		shoot_control.trigger_motor.trigger_given_current = 0;
		shoot_laser_off();
	}
	else if (shoot_control.shoot_mode == SHOOT_READY)
	{
		shoot_control.trigger_motor.trigger_speed_set = 0;
		ramp_calc(&shoot_control.fric_ramp, FRIC_START);
		shoot_control.trigger_motor.trigger_given_current = 0;
		shoot_laser_on();
	}
	else if (shoot_control.shoot_mode == SHOOT_START)
	{
		PID_calc(&shoot_control.trigger_motor_pid_cooling_heat, power_heat_data_t.shooter_id1_17mm_cooling_heat, robot_state_t.shooter_barrel_heat_limit * 0.9f);
		shoot_control.trigger_motor.trigger_speed_set = -shoot_control.trigger_motor_pid_cooling_heat.out;
		// 摩擦轮需要一个个斜波开启，不能同时直接开启，否则可能电机不转
		ramp_calc(&shoot_control.fric_ramp, FRIC_START);
		shoot_blocking_judgment();
		PID_calc(&shoot_control.trigger_motor_pid_speed, shoot_control.trigger_motor.trigger_speed, shoot_control.trigger_motor.trigger_speed_set);
		shoot_control.trigger_motor.trigger_given_current = shoot_control.trigger_motor_pid_speed.out;
		shoot_laser_on();
	}

	shoot_control.fric_motor[0].fric_speed_set = shoot_control.fric_ramp.out;
	shoot_control.fric_motor[1].fric_speed_set = -shoot_control.fric_ramp.out;
	PID_calc(&shoot_control.fric1_motor_pid_speed, shoot_control.fric_motor[0].fric_speed, shoot_control.fric_motor[0].fric_speed_set);
	PID_calc(&shoot_control.fric2_motor_pid_speed, shoot_control.fric_motor[1].fric_speed, shoot_control.fric_motor[1].fric_speed_set);
	shoot_control.fric_motor[0].fric_given_current = (int16_t)(shoot_control.fric1_motor_pid_speed.out);
	shoot_control.fric_motor[1].fric_given_current = (int16_t)(shoot_control.fric2_motor_pid_speed.out);
}

/**
  * @brief          射击状态机设置
  * @author			冉文治
  * @param			void
  * @retval         void
  */
static void shoot_set_mode(void)
{
	if (chassis_move.chassis_mode == CHASSIS_VECTOR_FOLLOW_GIMBAL_YAW || chassis_move.chassis_mode == CHASSIS_VECTOR_ROTATION)
	{
		if (switch_is_up(shoot_control.shoot_rc->rc.s[SHOOT_RC_MODE_CHANNEL]))
		{
				shoot_control.shoot_mode = SHOOT_START;
		}
		else if (switch_is_mid(shoot_control.shoot_rc->rc.s[SHOOT_RC_MODE_CHANNEL]))
		{
			shoot_control.shoot_mode = SHOOT_READY;
		}
		else if (switch_is_down(shoot_control.shoot_rc->rc.s[SHOOT_RC_MODE_CHANNEL]))
		{
			shoot_control.shoot_mode = SHOOT_STOP;
		}
	}
	else if (chassis_move.chassis_mode == CHASSIS_AUTO_FOLLOW_RADAR_YAW || chassis_move.chassis_mode == CHASSIS_AUTO_ROTATION)
	{

		if (switch_is_up(shoot_control.shoot_rc->rc.s[SHOOT_RC_MODE_CHANNEL]))
		{
			if (gimbal_behaviour == GIMBAL_CV && gimbal_control.cv_ctrl->is_fire == 1)
			{
				shoot_control.shoot_mode = SHOOT_START;
			}
			else if (gimbal_behaviour == GIMBAL_CV && gimbal_control.cv_ctrl->is_fire == 0)
			{
				shoot_control.shoot_mode = SHOOT_READY;
			}
			else
			{
				shoot_control.shoot_mode = SHOOT_READY;
			}
		}
		else if (switch_is_mid(shoot_control.shoot_rc->rc.s[SHOOT_RC_MODE_CHANNEL]) || switch_is_down(shoot_control.shoot_rc->rc.s[SHOOT_RC_MODE_CHANNEL]))
		{
			shoot_control.shoot_mode = SHOOT_STOP;
		}

	}
	else
	{
		shoot_control.shoot_mode = SHOOT_STOP;
	}
		
	shoot_control.shoot_last_mode = shoot_control.shoot_mode;
}

/**
  * @brief          射击数据更新
  * @author			冉文治
  * @param			void
  * @retval         void
  */
static void shoot_feedback_update(void)
{
	// 回传摩擦轮速度
	first_order_filter_cali(&shoot_control.fric_motor[0].fric_filter, shoot_control.fric_motor[0].fric_motor_measure->speed_rpm);
	first_order_filter_cali(&shoot_control.fric_motor[1].fric_filter, shoot_control.fric_motor[1].fric_motor_measure->speed_rpm);
	shoot_control.fric_motor[0].fric_speed = shoot_control.fric_motor[0].fric_filter.out;
    shoot_control.fric_motor[1].fric_speed = shoot_control.fric_motor[1].fric_filter.out;
	// 输出拨弹电流
	shoot_control.trigger_motor.trigger_speed = shoot_control.trigger_motor.trigger_motor_measure->speed_rpm;
}

/**
  * @brief          堵转处理
  * @author			卢俊烨
  *					冉文治
  * @param			void
  * @retval         void
  */
static void shoot_blocking_judgment(void)
{
	// 如果拨弹电机电流值大于8000
	if (abs(shoot_control.trigger_motor.trigger_given_current) > 9000 && shoot_control.trigger_motor.trigger_speed > -250)
	{
		// 卡弹标志位开始增加
		shoot_control.blocking_start_flag += gimbal_control.dt;
	}

	// 如果标志位大于200，即触发卡弹
	if (shoot_control.blocking_start_flag > 0.5f)
	{
		// 反转
		shoot_control.trigger_motor.trigger_speed_set = 2000.0f;
		// 卡弹结束标志位增加
		shoot_control.blocking_over_flag += gimbal_control.dt;
	}

	// 标志位误差触发处理
	if (shoot_control.blocking_over_flag >= 0.5f)
	{
		shoot_control.blocking_start_flag = 0;
		shoot_control.blocking_over_flag = 0;
		shoot_control.trigger_motor.trigger_speed_set = -shoot_control.trigger_motor_pid_cooling_heat.out;
	}
}

const shoot_control_t *get_shoot_motor_point(void)
{
	return &shoot_control;
}
